#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <unistd.h>
#include <sys/time.h>
#include "uvc_sdk.h"
#include <opencv2/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <gamma.h>
#include <fpscnt.hpp>

#ifdef ENABLE_GAMMA
uint8_t gamma_lut[256];
#endif

void cam1_callback(uvc_frame_t *frame, void *ptr)
{
    uint8_t * rgb_buf = (uint8_t *)malloc(640*480*3);
#ifndef ENABLE_RAW
    cvt_yuyv2bgr((uint8_t*)frame->data,rgb_buf,640,480);
    cv::Mat image_show = cv::Mat(cv::Size(640,480),CV_8UC3,rgb_buf);
#else
    #ifdef ENABLE_GAMMA
        struct timeval ts_start,ts_end;
        gettimeofday(&ts_start,NULL);
        gamma_correction(frame->data,640,480,gamma_lut);
        gettimeofday(&ts_end,NULL);
        double latency = (double)(ts_end.tv_sec*100000+ts_end.tv_usec)/1000 - (double)(ts_start.tv_sec*100000+ts_start.tv_usec)/1000;
        printf("Gamma latency is %lf \n",latency);
        
    #endif
        cv::Mat image_show = cv::Mat(cv::Size(640,480),CV_8UC1,frame->data);
#endif
    cv::imshow("Frame",image_show);
    cv::waitKey(1);
    struct timeval ts_cur;
    gettimeofday(&ts_cur,NULL);
    // printf("[cam1_callback]: %lf ms ",(double)(ts_cur.tv_sec*100000+ts_cur.tv_usec)/1000);
    // log_uvc_frame_format(frame->frame_format);
    free(rgb_buf);

    static FrequencyCounter fps;
    fps.tic();
    if (fps.count() % 100 == 1) {
        printf("Fisheyes : %lf Hz \n",fps.fps());
    }
}
int main(int argc , char * argv[])
{
#ifdef ENABLE_GAMMA
    generate_gamma_lut(gamma_lut,0.1);
#endif
    int uvc_format = UVC_FRAME_FORMAT_YUYV;


#ifdef ENABLE_RAW
    uvc_format = UVC_FRAME_FORMAT_NV12;
#endif
    UVC_UTILS_DEVICE cam1;
    cam1.format      = uvc_format;
    cam1.frameWidth  = 640;
    cam1.frameHeight = 480;
    cam1.targetFps   = 30;
    cam1.pid         = 0x2bdf;
    cam1.vid         = 0x028a;
    cam1.cb          = cam1_callback;
    
    init_uvc_camera_device(&cam1);
    
    start_uvc_camera_device(&cam1);
    
    sleep(10000);
    free_uvc_camera_device(&cam1);
}